#include "ChargeControl.h"
#include "TemperatureStuff.h"
#include "MyCan.h"
#include "userConfig.h"
#include "Serial.h"
#include <can.h>
#include <stdlib.h>


#define CHARGE_COMMAND_BYTE 0
#define STOP_CHARGE_COMMAND_BYTE 1

const int TIMEOUTS_BEFORE_RESET = 20;
int timeouts;

unsigned char IsCommunicating;
unsigned char IsCharging;

unsigned char ChargeCommandData[8];
unsigned char ChargeStatusData[8];

unsigned char ChargerSA;

unsigned char SavedErrorState;

ChargeStates ChargeState;

void DetermineChargeCommandByte(unsigned int chargeVoltage);
static unsigned int ParseChargerVoltage();
static unsigned int ParseChargerCurrent();
static unsigned char ParseChargerStatus();

ChargeStates isWarmEnough();
ChargeStates isExternalWarmEnough();
ChargeStates isInternalWarmEnough();

void CC_ReloadVoltageParameter()
{
    int encodedTermVoltage = (GetTerminationVoltage() + 20);
    ChargeCommandData[0] = encodedTermVoltage >> 8;  //voltage high
    ChargeCommandData[1] = encodedTermVoltage & 0xff;  //voltage low
}

void CC_ReloadCurrentParameter()
{
    int encodedTermCurrent = GetCurrentCommand();
    ChargeCommandData[2] = encodedTermCurrent >> 8;  //current high
    ChargeCommandData[3] = encodedTermCurrent & 0xff;  //current low
}
void CC_InitVariables()
{
    CC_ReloadVoltageParameter();
    CC_ReloadCurrentParameter();
    
    ChargeCommandData[4] = 0x00;
    ChargeCommandData[5] = 0x00;
    ChargeCommandData[6] = 0x00;
    ChargeCommandData[7] = 0x00;

    IsCommunicating = 0;
    IsCharging = 0;

    ChargerSA = 0;

    timeouts = TIMEOUTS_BEFORE_RESET;
    ChargeState = NOT_CONNECTED;
    SavedErrorState = 0;
}


void CC_HandleChargerStatusMessage()
{
    if(GetMasterMode())
    {
        ChargerSA = CAN1GetSA();
        CAN1ReceiveMessage(ChargeStatusData, 8, 0);
//        if(DoesSidEidMatchMessageBase(SID, EID, CHARGER_STATUS_MESSAGE_BASE))
        {
            if(ParseChargerStatus() != 0)
            {
                SavedErrorState = ParseChargerStatus();
            }
            timeouts = TIMEOUTS_BEFORE_RESET;
            DetermineChargeCommandByte(ParseChargerVoltage());
            uint32_t identifer = StitchDAIntoFullIdentifier(CHARGE_COMMAND_MESSAGE_BASE, ChargerSA);
            SendCANMessage(identifer, ChargeCommandData, 0);
        }
    }
    CAN1SetRXMode(0, CAN_RXFUL_CLEAR & CAN_BUF0_DBLBUFFER_EN);
}

void CC_HandleOtherChargeCommandMessage()
{
    if(GetMasterMode())
    {
        PutString("Backing off because another master is present \r");
    }
    __builtin_disi(0x3FFF);
    SetMasterMode(0);
    __builtin_disi(0);
}

void CC_EnableMasterMode()
{
    SendCANMessage(CHARGE_COMMAND_MESSAGE_BASE, ChargeCommandData, 2);
}
unsigned int ParseChargerVoltage()
{
    return ((unsigned int)ChargeStatusData[0] << 8) + ChargeStatusData[1];
}

unsigned int ParseChargerCurrent()
{
    return ((unsigned int)ChargeStatusData[2] << 8) + ChargeStatusData[3];
}

unsigned char ParseChargerStatus()
{
    return ChargeStatusData[4];
}
void DetermineChargeCommandByte(unsigned int chargeVoltage)
{
    if(ChargeState != CHARGE_COMPLETE)
    {
        ChargeState = isWarmEnough();
    }

    if(chargeVoltage > GetTerminationVoltage())
    {
        ChargeState = CHARGE_COMPLETE;
    }
    
    if(ChargeState == CHARGING_NORMAL)
    {
        ChargeCommandData[4] = CHARGE_COMMAND_BYTE;
    }
    else
    {
        ChargeCommandData[4] = STOP_CHARGE_COMMAND_BYTE;
    }
}

void CC_HandleTimerExpiration()
{
    if(timeouts)
    {
        timeouts--;
    }
    else
    {
        ChargeState = NOT_CONNECTED;
        IsCommunicating = 0;
    }
}

ChargeStates isInternalWarmEnough()
{
    
    ChargeStates retVal = INTERNAL_MISSING_SENSOR;
    if(DevicesConnected >= GetInternalNodeChargeCount())
    {
        retVal = CHARGING_NORMAL;
        int loopVar;
        for(loopVar = 0; loopVar < DevicesConnected; loopVar++)
        {
            if(TemperatureReadings[loopVar] < GetTemperatureThreshold())
            {
                retVal = INTERNAL_TOO_COLD;
                break;
            }
        }
    }
    return retVal;
}

ChargeStates isExternalWarmEnough()
{
    ChargeStates retVal = OTHER_MISSING_SENSOR;
    if(ExternalDeviceCount >= GetExternalNodeChargeCount())
    {
        retVal = CHARGING_NORMAL;
        int loopVar;
        const unsigned char MINIMUM_TEMPERATURE_FOR_CHARGING_CAN = ConvertCelciusToCAN(GetTemperatureThreshold());
        for(loopVar = 0; loopVar < ExternalDeviceCount && CHARGING_NORMAL == retVal; loopVar++)
        {
            int innerLoopVar;
            for(innerLoopVar = 0; innerLoopVar < 7; innerLoopVar++)
            {
                unsigned char tempReading = ExternalData[loopVar].tempValues[innerLoopVar];
                if(tempReading == 0xfe)
                {
                    retVal = OTHER_MISSING_SENSOR;
                    break;
                }
                if(tempReading < MINIMUM_TEMPERATURE_FOR_CHARGING_CAN)
                {
                    retVal = OTHER_TOO_COLD;
                    break;
                }

            }
        }
    }
    return retVal;
}

ChargeStates isWarmEnough()
{
    ChargeStates retVal = isInternalWarmEnough();
    if(retVal == CHARGING_NORMAL)
    {
        retVal = isExternalWarmEnough();
    }
    return retVal;
}

void CC_PrintChargerStatus()
{
    if(ChargeState == NOT_CONNECTED)
    {
        PutString("No Charger to report on\r");
    }
    else
    {
        __builtin_disi(0x3FFF);
        unsigned int chargerCurrent = ParseChargerCurrent();
        unsigned int chargerVoltage = ParseChargerVoltage();
        unsigned char chargerStatus = ParseChargerStatus();
        unsigned char previousError = SavedErrorState;
        unsigned char chargerSA = ChargerSA;
        __builtin_disi(0);

        PutString("Charger at ");
        hex_ascii(chargerSA);
        PutString("\rCharget Voltage ");
        char buffer[10];
        itoa(buffer, chargerVoltage, 10);
        PutString(buffer);
        PutString("\rCharger Current ");
        itoa(buffer, chargerCurrent, 10);
        PutString(buffer);
        PutString("\rCurrent Error Codes: ");
        hex_ascii(chargerStatus);
        PutString("\rLast Error Code:");
        hex_ascii(previousError);
        Putchar('\r');
    }
};